package simpleGeom;

import java.util.ArrayList;
import java.util.Vector;

import processing.core.PGraphics;
import processing.core.PVector;

public class Plane {
	Vec3D normal;// normal;
	float d0 = 0;// distanceToOrigin;
	// hessian normal form

	public Plane(HasPos3D p, HasPos3D normal) {
		// this.p=new Vec3D(p);
		this.normal = new Vec3D(normal);
		this.normal.normalize();
		d0 = -this.normal.dot(p);

	}

	public Plane(HasPos3D p0, HasPos3D p1, HasPos3D p2) {
		// this.p=new Vec3D(p);
		this.normal = Vec3D.getNormal(p0, p1, p2);
		this.normal.normalize();
		d0 = -this.normal.dot(p0);
	}

	public float getDistanceSigned(PVector v) {
		return getDistanceSigned(v.x, v.y, v.z);
	}

	public float getDistanceSigned(HasPos3D v) {
		return getDistanceSigned(v.getX(), v.getY(), v.getZ());
	}

	public float getDistanceSigned(float x, float y, float z) {
		return normal.dot(x, y, z) + d0;
	}

	public Vec3D intersectWithRay(HasPos3D v0, HasPos3D v1) {
		return intersectWithLine(v0.getX(), v0.getY(), v0.getZ(), v1.getX(),
				v1.getY(), v1.getZ());
	}

	public Vec3D getPlanePoint(Vec3D v) {
		Vec3D normal = getNormal();
		return intersectWithLine(v.getX(), v.getY(), v.getZ(), v.getX()
				+ normal.getX(), v.getY() + normal.getY(),
				v.getZ() + normal.getZ());
	}

	public Vec3D intersectWithLine(float ax, float ay, float az, float bx,
			float by, float bz) {
		float d0 = getDistanceSigned(ax, ay, az);
		float d1 = getDistanceSigned(bx, by, bz);
		Vec3D v = new Vec3D(bx - ax, by - ay, bz - az);
		v.mult(d0 / (d0 - d1));
		v.setPosition(v.x + ax, v.y + ay, v.z + az);
		// Q = P0 +(d0/(d0 −d1))(P1 −P0)
		return v;
	}

	public Mesh mirror(Mesh mesh) {
		Mesh copyMesh = mesh.getCopy();
		for (int i = 0; i < copyMesh.getNNodes(); i++) {
			MeshNode n = copyMesh.getNode(i);
			Vec3D planeP = getPlanePoint(n);
			Vec3D v = Vec3D.sub(planeP, n);
			v.mult(2);
			n.add(v);
		}
		return copyMesh;
	}
	
	public Mesh cutMesh(Mesh mesh) {
		Mesh copyMesh = mesh.getCopyNodes();
		ArrayList<MeshFace> cutFaces = new ArrayList<MeshFace>();
		ArrayList<MeshEdge> cutEdges = new ArrayList<MeshEdge>();
		for (int i = 0; i < mesh.getNNodes(); i++) {
			MeshNode n = mesh.getNode(i);
			n.comment = 0;
		}
		for (int i = 0; i < mesh.getNNodes(); i++) {
			MeshNode n = mesh.getNode(i);
			float d1 = getDistanceSigned(n.getX(), n.getY(), n.getZ());
			if (d1 >= 0) {
				n.comment = 1;
			}
		}
		for (int i = 0; i < mesh.getNFaces(); i++) {
			MeshFace f = mesh.getFace(i);
			int nInside = 0;
			for (int ii = 0; ii < f.getNNodes(); ii++) {
				MeshNode n = f.getNode(ii);
				if (n.comment == 1) {
					nInside++;
				}
			}
			if (nInside > 2) {
				Vector<MeshNode> newNodes = new Vector<MeshNode>();
				for (int ii = 0; ii < f.getNNodes(); ii++) {
					MeshNode cNode = f.getNode(ii);
					MeshNode newNode = copyMesh.getNode(cNode.getId());
					newNodes.add(newNode);
				}
				copyMesh.addFace(newNodes);
			} else if (nInside > 0) {

				for (int ii = 0; ii < f.getNNodes(); ii++) {
					MeshNode n1 = f.getNode(ii);
					MeshNode n2 = f.getNode(ii + 1);
					MeshNode n3 = f.getNode(ii + 2);
					if (n1.comment == 0 && n2.comment == 1 && n3.comment == 0) {
						Vec3D newV1 = intersectWithRay(n1, n2);
						Vec3D newV2 = intersectWithRay(n2, n3);

						Vector<MeshNode> splitNodes = new Vector<MeshNode>();
						MeshNode newNode1 = copyMesh.addNode(newV1.getX(),
								newV1.getY(), newV1.getZ());
						MeshNode newNode2 = copyMesh.addNode(newV2.getX(),
								newV2.getY(), newV2.getZ());

						splitNodes.add(newNode1);
						splitNodes.add(copyMesh.getNode(n2.getId()));
						splitNodes.add(newNode2);
						copyMesh.addFace(splitNodes);
						break;
					} else if (n1.comment == 0 && n2.comment == 1
							&& n3.comment == 1) {
						Vec3D newV1 = intersectWithRay(n1, n2);
						Vec3D newV2 = intersectWithRay(n3, n1);

						MeshNode newNode1 = copyMesh.addNode(newV1.getX(),
								newV1.getY(), newV1.getZ());
						MeshNode newNode2 = copyMesh.addNode(newV2.getX(),
								newV2.getY(), newV2.getZ());
						Vector<MeshNode> splitNodes1 = new Vector<MeshNode>();
						splitNodes1.add(newNode1);
						splitNodes1.add(copyMesh.getNode(n2.getId()));
						splitNodes1.add(copyMesh.getNode(n3.getId()));
						copyMesh.addFace(splitNodes1);

						Vector<MeshNode> splitNodes2 = new Vector<MeshNode>();
						splitNodes2.add(copyMesh.getNode(n3.getId()));
						splitNodes2.add(newNode2);
						splitNodes2.add(newNode1);
						copyMesh.addFace(splitNodes2);
						break;
					}
				}
			}
		}
		copyMesh.removeUnusedNodes();
		return copyMesh;
	}

	public void getIntersections(HasPos3D p1, HasPos3D p2, HasPos3D p3) {

	}

	public Vec3D getNormal() {
		return normal;
	}

	public void setNormal(Vec3D normal) {
		this.normal = normal;
	}

	final static float tol = 0.0000001f;

	public static HasPos3D[][] intersectTriPlane(HasPos3D a1, HasPos3D a2,
			HasPos3D a3, HasPos3D b1, HasPos3D b2, HasPos3D b3) {
		return intersectTriPlane(a1, a2, a3, new Plane(b1, b2, b3));
	}

	public static HasPos3D[][] intersectTriPlane(HasPos3D a1, HasPos3D a2,
			HasPos3D a3, Plane plane) {
		HasPos3D[] tri = new HasPos3D[] { a1, a2, a3 };
		float[] dA = new float[3];
		for (int i = 0; i < tri.length; i++) {
			float dist = plane.getDistanceSigned(tri[i]);
			dA[i] = dist;
		}
		HasPos3D[] iPts = new HasPos3D[2];
		int[] ids = new int[2];

		int iID = 0;
		for (int i = 0; i < 3; i++) {
			int i2 = (i + 1) % 3;
			float dist1 = dA[i];
			float dist2 = dA[i2];
			if (Math.abs(dist1) < tol) {
				if (Math.abs(dist2) > tol) {
					iPts[iID] = tri[i];
					ids[iID] = i;
					iID++;
				}
			} else {
				if ((dist1 < -tol && dist2 > tol)
						|| (dist1 > tol && dist2 < -tol)) {
					iPts[iID] = plane.intersectWithRay(tri[i], tri[i2]);
					ids[iID] = i;
					iID++;
				}
			}
		}
		if (iID != 2)
			return null;
		HasPos3D int1 = iPts[0];
		int id1 = ids[0];
		HasPos3D int2 = iPts[1];
		int id2 = ids[1];
		if (int1 == tri[id1]) {
			HasPos3D[][] trias = new HasPos3D[2][3];
			trias[0] = new HasPos3D[] { int1, tri[(id1 + 1) % 3], int2 };
			trias[1] = new HasPos3D[] { int1, int2, tri[(id1 + 2) % 3] };
			return trias;
		} else if (int2 == tri[id2]) {
			HasPos3D[][] trias = new HasPos3D[2][3];
			trias[0] = new HasPos3D[] { int2, tri[(id2 + 1) % 3], int1 };
			trias[1] = new HasPos3D[] { int2, int1, tri[(id2 + 2) % 3] };
			return trias;
		} else {
			HasPos3D[][] trias = new HasPos3D[3][3];
			trias[0] = new HasPos3D[] { int1, int2, tri[(id2 + 1) % 3] };
			trias[1] = new HasPos3D[] { int2, int1, tri[(id1 + 1) % 3] };
			if (id1 + 1 == id2) {
				trias[2] = new HasPos3D[] { int1, tri[(id2 + 1) % 3],
						tri[(id2 + 2) % 3] };
			} else {
				trias[2] = new HasPos3D[] { tri[(id1 + 1) % 3],
						tri[(id1 + 2) % 3], int2 };
			}
			return trias;
		}
	}

	public static HasPos3D[][] cropTriPlane(HasPos3D a1, HasPos3D a2,
			HasPos3D a3, Plane plane) {
		HasPos3D[] tri = new HasPos3D[] { a1, a2, a3 };
		float[] dA = new float[3];
		for (int i = 0; i < tri.length; i++) {
			float dist = plane.getDistanceSigned(tri[i]);
			dA[i] = dist;
		}
		HasPos3D[] iPts = new HasPos3D[2];
		int[] ids = new int[2];

		int iID = 0;
		Vector<HasPos3D> rightPts = new Vector<HasPos3D>(4);
		Vector<HasPos3D> leftPts = new Vector<HasPos3D>(4);
		for (int i = 0; i < 3; i++) {
			int i2 = (i + 1) % 3;
			float dist1 = dA[i];
			float dist2 = dA[i2];
			if (dist1 <= 0) {
				leftPts.add(tri[i]);
			}
			if (dist1 >= 0) {
				rightPts.add(tri[i]);
			}
			if (dist1 < -tol && dist2 > tol || dist1 > tol && dist2 < -tol) {
				Vec3D v = plane.intersectWithRay(tri[i], tri[i2]);
				leftPts.add(v);
				rightPts.add(v);
			}
		}
		HasPos3D[] lPts = new HasPos3D[leftPts.size()];
		for (int i = 0; i < leftPts.size(); i++) {
			lPts[i] = leftPts.get(i);
		}
		HasPos3D[] rPts = new HasPos3D[rightPts.size()];
		for (int i = 0; i < rightPts.size(); i++) {
			rPts[i] = rightPts.get(i);
		}
		HasPos3D[][] pts = new HasPos3D[2][];
		pts[0] = lPts;
		pts[1] = rPts;
		return pts;
	}
}
